#include <cassert>
#include "matrix.h"
#include <math.h>
namespace gemren
{
	typedef float float_type;
	matrix4x4::matrix4x4( float_type a1, float_type a2, float_type a3, float_type a4,
						  float_type b1, float_type b2, float_type b3, float_type b4,
						  float_type c1, float_type c2, float_type c3, float_type c4,
						  float_type d1, float_type d2, float_type d3, float_type d4)
	{
		mat[ 0] = a1; mat[ 1] = a2; mat[ 2] = a3; mat[ 3] = a4; 
		mat[ 4] = b1; mat[ 5] = b2; mat[ 6] = b3; mat[ 7] = b4; 
		mat[ 8] = c1; mat[ 9] = c2; mat[10] = c3; mat[11] = c4; 
		mat[12] = d1; mat[13] = d2; mat[14] = d3; mat[15] = d4; 
	}
	matrix4x4::matrix4x4()
	{
		for(int i = 0; i<16; ++i)
			mat[i] = 0;
	}
	float_type& matrix4x4::operator()(int row,int col)
	{
		assert(row>=0);
		assert(row<4);
		assert(col>=0);
		assert(col<4);
		return mat[(row<<2)+col];
	}
	const float_type& matrix4x4::operator()(int row,int col)const
	{
		assert(row>=0);
		assert(row<4);
		assert(col>=0);
		assert(col<4);
		return mat[(row<<2)+col];
	}
	matrix4x4 matrix4x4::operator *(const matrix4x4& m) const
	{
		matrix4x4 ret;
		for(int r = 0; r<4; ++r)
			for(int c = 0; c<4; ++c)
				for(int i = 0; i<4; ++i)
				//TODO without operators
					ret(r,c) += mat[(r<<2)+i] * m(i,c);
		return ret;
	}
	matrix4x4::coord matrix4x4::operator*(const coord& v) const
	{
		return coord(   v.x*mat[ 0] + v.y*mat[ 1]+ v.z*mat[ 2] + mat[ 3],
						v.x*mat[ 4] + v.y*mat[ 5]+ v.z*mat[ 6] + mat[ 7],
						v.x*mat[ 8] + v.y*mat[ 9]+ v.z*mat[10] + mat[11]);
	}
					

	matrix4x4 matrix4x4::identity()
	{
		return matrix4x4(   1,0,0,0,
							0,1,0,0,
							0,0,1,0,
							0,0,0,1);
	}
	
	matrix4x4 matrix4x4::linear_transformation(const coord& b1, const coord& b2, const coord& b3)
	{
		return matrix4x4(
			b1.x, b2.x, b3.x, 0,
			b1.y, b2.y, b3.y, 0,
			b1.z, b2.z, b3.z, 0,
			0,    0,    0,    1
		);
	}

	matrix4x4 matrix4x4::move(float_type x, float_type y, float_type z)
	{
		return matrix4x4(   1,0,0,x,
							0,1,0,y,
							0,0,1,z,
							0,0,0,1);
	}
	matrix4x4 matrix4x4::scale(float_type x, float_type y, float_type z)
	{
		return matrix4x4(   x,0,0,0,
							0,y,0,0,
							0,0,z,0,
							0,0,0,1);

	}
	matrix4x4 matrix4x4::rotateX(float_type angle)
	{
		float_type c = cos(angle);
		float_type s = sin(angle);
		return matrix4x4(   1,0, 0,0,
							0,c,-s,0,
							0,s, c,0,
							0,0, 0,1);
	}
	matrix4x4 matrix4x4::rotateY(float_type angle)
	{
		float_type c = cos(angle);
		float_type s = sin(angle);
		return matrix4x4(    c,0,s,0,
							 0,1,0,0,
							-s,0,c,0,
							 0,0,0,1);
	}
	matrix4x4 matrix4x4::rotateZ(float_type angle)
	{
		float_type c = cos(angle);
		float_type s = sin(angle);
		return matrix4x4(   c,-s,0,0,
							s, c,0,0,
							0, 0,1,0,
							0, 0,0,1);
	}

	void matrix4x4::multvec(coord& c) const
	{
		c.x = c.x*mat[ 0] + c.y*mat[ 1]+ c.z*mat[ 2];
		c.y = c.x*mat[ 4] + c.y*mat[ 5]+ c.z*mat[ 6];
		c.z = c.x*mat[ 8] + c.y*mat[ 9]+ c.z*mat[10];
	}
	matrix4x4::coord matrix4x4::transform_vector(const coord& c) const
	{ //transforms normal vector of a plane
		coord other; //will be perpendicular to c
		if(c.y< -EPSILON || c.y > EPSILON) other = coord(-1,(c.x+c.z)/c.y,-1);
		else if(c.z< -EPSILON || c.z > EPSILON) other = coord(-1,-1, (c.x+c.y)/c.z);
		else if(c.x< -EPSILON || c.x > EPSILON)other = coord((c.y+c.z)/c.x,-1,-1);
		else return c; //is zero

		coord second = embree::cross(c,other);

		multvec(other);
		multvec(second);
		coord ret(embree::cross(other,second));
		return ret;
	}
}